/*
 *Copyright (c) 2024 Black Sesame Technologies
 *
 *Licensed under the Apache License, Version 2.0 (the "License");
 *you may not use this file except in compliance with the License.
 *You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 *Unless required by applicable law or agreed to in writing, software
 *distributed under the License is distributed on an "AS IS" BASIS,
 *WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *See the License for the specific language governing permissions and
 *limitations under the License.
*/

#include "can_output.h"
#include "VCan_Types.h"
#include "eth2can.h"
#include "vcan.h"
#include "can_msg.h"
#include "can_config.h"
#include "../hscg_flexcan/FlexCAN_Ip.h"
#include "hscg_firmware_packet_statistic.h"
#include "hscg_firmware_statistic.h"

/**
 * Each CAN controller has 56 TX MBs, with 55 MBs bound to specific filters of a CAN interface.
 * One MB is reserved for other filters due to the number of filters exceeding the number of MBs.
 */
#define HSCG_CAN_CONFIG_MAXRXMB_Z1 (HSCG_CAN_CONFIG_MAXRXMB - 1)
#define HSCG_CAN_MAX_MB_COUNT_Z1 (HSCG_CAN_MAX_MB_COUNT - 1)

#define HSCG_CAN_TX_QUE_MB_IDX 55

static sint8 can_tx_mb_map[LLCG_CONFIG_MAXCTRL_COUNT][HSCG_CAN_CONFIG_MAXRXMB_Z1][LLCG_CONFIG_MAXCTRL_COUNT] = {-1};
static sint8 mb_pool[LLCG_CONFIG_MAXCTRL_COUNT][HSCG_CAN_MAX_MB_COUNT_Z1] = {-1};
static uint8 mb_pool_slide_index[LLCG_CONFIG_MAXCTRL_COUNT] = {0};

SQ_Buffer* can2x_sq_buffer = (SQ_Buffer *) CAN2X_SQ_ADDR;
CAN2X* can2x_buffers[SQ_BUFFER_COUNT];

static volatile uint32* can_mb_paddr[LLCG_CONFIG_MAXCTRL_COUNT];


/**
 * @brief Converts CANFD data length to DLC.
 *
 * @param datalen The CANFD data length.
 * @return The DLC value.
 */
uint8 utils_canfd_datalen_to_dlc(uint8 datalen)
{
    if (datalen <= 8)
        return datalen;

    switch (datalen) {
        case 12: return 9;
        case 16: return 10;
        case 20: return 11;
        case 24: return 12;
        case 32: return 13;
        case 48: return 14;
        case 64: return 15;
        default: hprintf(DEBUG, "ERROR: utils_canfd_datalen_to_dlc datalen:%d\n", datalen); return 0;
    }
}

/**
 * @brief Computes the length between two ring pointers.
 *
 * @param first The first pointer.
 * @param second The second pointer.
 * @param size The size of each element.
 * @param ring_start The start of the ring buffer.
 * @param ring_end The end of the ring buffer.
 * @return The length between the two pointers.
 */
uint32
util_compute_len_between_two_ring_ptr(uint8* first, uint8* second, size_t size, uint8* ring_start, uint8* ring_end)
{
    if ((first < ring_start) || (first > ring_end) || (second < ring_start) || (second > ring_end)) {
        hprintf(DEBUG, "util_compute_len_between_two_ring_ptr parameter error! 0x%x 0x%x 0x%d 0x%x 0x%x\n", first,
                second, size, ring_start, ring_end);
        return 0;
    }

    return (second >= first) ? (second - first) / size : ((second - ring_start) + (ring_end - first)) / size;
}

static uint32 Get_Gccsidr(void)
{
	uint32 ccsidr;

	/* Read current CP15 Cache Size ID Register */
	asm volatile ("mrc p15, 1, %0, c0, c0, 0" : "=r" (ccsidr));
	return ccsidr;
}

static void DcacheCleanInvalRange(uint32 start, uint32 stop, uint32 line_len)
{
	uint32 mva;

	/* Align start to cache line boundary */
	start &= ~(line_len - 1);
	for (mva = start; mva < stop; mva = mva + line_len) {
		/* DCCIMVAC - Clean & Invalidate data cache by MVA to PoC */
		asm volatile ("mcr p15, 0, %0, c7, c14, 1" : : "r" (mva));
	}
}

static void DcacheInvalRange(uint32 start, uint32 stop, uint32 line_len)
{
	uint32 mva;

	/* Align start to cache line boundary */
	start &= ~(line_len - 1);
	for (mva = start; mva < stop; mva = mva + line_len) {
		/* DCIMVAC - Invalidate data cache by MVA to PoC */
		asm volatile ("mcr p15, 0, %0, c7, c6, 1" : : "r" (mva));
	}
}
void DcacheMaintRange(uint32 start, uint32 stop, CacheOpsType range_op)
{
	uint32 line_len, ccsidr;

	ccsidr = Get_Gccsidr();
	line_len = ((ccsidr & CCSIDR_LINE_SIZE_MASK) >>
			CCSIDR_LINE_SIZE_OFFSET) + 2;
	/* Converting from words to bytes */
	line_len += 2;
	/* converting from log2(linelen) to linelen */
	line_len = 1 << line_len;
	switch (range_op) {
	case DCACHE_CLEAN_INVAL_RANGE:
		DcacheCleanInvalRange(start, stop, line_len);
		break;
	case DCACHE_INVAL_RANGE:
		DcacheInvalRange(start, stop, line_len);
		break;
	}

	/* DSB to make sure the operation is complete */
	__asm__("DSB");
}

/**
 * @brief Update tx queue mb address.
 */
void can_que_mb_addr_init(uint32 controller_id)
{
    can_mb_paddr[controller_id] = FlexCAN_GetMsgBuffRegion(Flexcan_Ip_apxBase[controller_id], HSCG_CAN_TX_QUE_MB_IDX);
}

/**
 * @brief Initializes the CAN output.
 */
void can_output_init(void)
{
    int ret;

    can_tx_mb_map_init();
    can_txque_init();

    /* Initialize the sq_buffer */
    for (int i = 0; i < SQ_BUFFER_COUNT; i++) {
        can2x_buffers[i] = (CAN2X *) (CAN2X_DATA_BUFFER_ADDR + i * sizeof(CAN2X));
    }
    ret = SQ_Buffer_Init(can2x_sq_buffer, (void **)can2x_buffers, sizeof(CAN2X), SQ_BUFFER_COUNT);
    if (ret != 0)
        hprintf(DEBUG, "SQ_Buffer_Init failed ret:%d\n", ret);
    else {
        hprintf(DEBUG, "SQ_Buffer_Init success at 0x%x ~ 0x%x(SQ_buffer:%d) buffer_addr 0x%x~0x%x sizeof(CAN2X_DATA):%d \n", 
            CAN2X_SQ_ADDR, CAN2X_SQ_ADDR+sizeof(SQ_Buffer), sizeof(SQ_Buffer),
            CAN2X_DATA_BUFFER_ADDR, CAN2X_DATA_BUFFER_ADDR + SQ_BUFFER_COUNT*sizeof(CAN2X),
            sizeof(CAN2X));
        for(int i=0;i<SQ_BUFFER_COUNT;i++) {
            can2x_sq_buffer->buffers_phy_addr64[i] = (uint64)can2x_buffers[i];
        }
    }
}

static void can_build_tx_msg(Flexcan_Ip_MsgBuffType *msg, uint32 msg_id,
                             uint32 cs, const uint8 *data, uint32 dataLen)
{
    msg->cs = cs;
    msg->msgId = msg_id;
    msg->dataLen = dataLen;

    /**
     * X2CAN 发送前，需要转换大小端
     *
     */
    uint8 dw = (dataLen % 4 == 0 ? (dataLen / 4) : (dataLen / 4 + 1));
    for (uint8 i = 0; i < dw; i++) {
        uint32 *src = &(data[i * 4]);
        uint32 *dst = &(msg->data[i * 4]);
        *dst = __builtin_bswap32(*src);
    }
}

/**
 * @brief Check tx mb is available.
 */
static bool can_que_mb_is_available(uint32 controller_id)
{
    uint32 cs_code;

    if (!can_mb_paddr[controller_id]) {
        can_mb_paddr[controller_id] = FlexCAN_GetMsgBuffRegion(Flexcan_Ip_apxBase[controller_id], HSCG_CAN_TX_QUE_MB_IDX);
        if (!can_mb_paddr[controller_id]) {
            return 0;
        }
    }

    cs_code = ((*can_mb_paddr[controller_id]) & FLEXCAN_IP_CS_CODE_MASK) >> FLEXCAN_IP_CS_CODE_SHIFT;

    /* 检查MB是否可用 */
    if ((cs_code != FLEXCAN_TX_INACTIVE) &&
        // 0x0 表示第一次初始化
        (cs_code != 0x0) &&
        // fix:修复发送了RTR帧后，无法再发送其他帧的问题。
        // 参考flexcan_ug手册，如果发送了RTR报文，会自动的把这个MB转成RX_EMPTY
        (cs_code != FLEXCAN_RX_EMPTY)) {
        return 0;
    }

    return 1;
}

/**
 * @brief Runs the CAN output process.
 */
void can_output_run(void)
{
    /* Check if there is data in the transmission queue */
    Flexcan_Ip_MsgBuffType *tx_msg;
    sint32 ret;
    uint32 i;

    for (i = 0; i < LLCG_CONFIG_MAXCTRL_COUNT; i++) {
        if (can_txque_is_empty(i))
            continue;

        if (!can_que_mb_is_available(i))
            continue;

        ret = can_txque_dequeue(i, &tx_msg);
        if (ret != 0)
            continue;

        STATISTICS_UPDATE_PACKET_ETH2CAN_TIMESTAMP(TIMESTAMP_PACKET_ETH2CAN_SENDTO_CAN_CONTROLLER_START);
        FlexCAN_Transmit(Flexcan_Ip_apxBase[i], HSCG_CAN_TX_QUE_MB_IDX, tx_msg->cs,
                         tx_msg->msgId, tx_msg->data, tx_msg->dataLen);
        can_msg_free(tx_msg);
        STATS_INC_CAN_TX(i);
    }
}

/**
 * @brief Initializes the CAN TX MB mapping and pool.
 * This function sets all TX MB mappings to -1 and initializes the MB pool.
 */
void can_tx_mb_map_init(void)
{
    sint32 i, j;

    memset(can_tx_mb_map, -1, sizeof(can_tx_mb_map));
    memset(mb_pool_slide_index, 0, sizeof(mb_pool_slide_index));

    for (i = 0; i < LLCG_CONFIG_MAXCTRL_COUNT; ++i) {
        for (j = 0; j < HSCG_CAN_MAX_MB_COUNT_Z1; ++j) {
            mb_pool[i][j] = j;
        }
    }
}

/**
 * @brief Allocates a TX MB index for a specified destination CAN controller.
 *
 * @param dest_ctrl_idx The index of the destination CAN controller.
 * @return The allocated MB index, or -1 if allocation fails.
 */
static sint8 can_tx_mb_index_alloc(uint32 dest_ctrl_idx)
{
    sint8 mb_index;

    if ((dest_ctrl_idx >= LLCG_CONFIG_MAXCTRL_COUNT) ||
        (mb_pool_slide_index[dest_ctrl_idx] >= HSCG_CAN_MAX_MB_COUNT_Z1)) {
        return -1;
    }

    mb_index = mb_pool[dest_ctrl_idx][mb_pool_slide_index[dest_ctrl_idx]];

    mb_pool[dest_ctrl_idx][mb_pool_slide_index[dest_ctrl_idx]] = -1;
    mb_pool_slide_index[dest_ctrl_idx] += 1;

    return mb_index;
}

/**
 * @brief Frees a previously allocated TX MB index for a specified destination CAN controller.
 *
 * @param dest_ctrl_idx The index of the destination CAN controller.
 * @param mb_idx The MB index to be freed.
 * @return 0 on success, -1 if parameters are invalid, or -2 if mb_idx is out of range.
 */
static sint32 can_tx_mb_index_free(uint32 dest_ctrl_idx, sint8 mb_idx)
{
    if ((dest_ctrl_idx >= LLCG_CONFIG_MAXCTRL_COUNT) || (mb_pool_slide_index[dest_ctrl_idx] == 0)) {
        return CAN_TX_ERR_PARA;
    }
    if ((mb_idx < 0) || (mb_idx >= HSCG_CAN_MAX_MB_COUNT_Z1))
        return CAN_TX_ERR_PARA;

    mb_pool_slide_index[dest_ctrl_idx] -= 1;
    mb_pool[dest_ctrl_idx][mb_pool_slide_index[dest_ctrl_idx]] = mb_idx;
    return CAN_TX_ERR_OK;
}

/**
 * @brief Sets a TX MB mapping for a specified source CAN controller and filter to a destination CAN controller.
 *
 * @param src_ctrl_idx The index of the source CAN controller.
 * @param src_filter_id The ID of the source filter.
 * @param dest_ctrl_idx The index of the destination CAN controller.
 * @return The allocated MB index, or -1 if the allocation fails.
 */
sint32 can_tx_mb_map_set(uint32 src_ctrl_idx, uint32 src_filter_id, uint32 dest_ctrl_idx)
{
    sint8 mb_index;

    if ((src_ctrl_idx >= LLCG_CONFIG_MAXCTRL_COUNT) || (src_filter_id >= HSCG_CAN_CONFIG_MAXRXMB_Z1) ||
        (dest_ctrl_idx >= LLCG_CONFIG_MAXCTRL_COUNT)) {
        return CAN_TX_ERR_PARA;
    }

    mb_index = can_tx_mb_index_alloc(dest_ctrl_idx);
    if (mb_index < 0) {
        return CAN_TX_ERR_NOMB;
    }

    can_tx_mb_map[src_ctrl_idx][src_filter_id][dest_ctrl_idx] = mb_index;
    return mb_index;
}

/**
 * @brief Clears a TX MB mapping for a specified source CAN controller and filter to a destination CAN controller.
 *
 * @param src_ctrl_idx The index of the source CAN controller.
 * @param src_filter_id The ID of the source filter.
 * @param dest_ctrl_idx The index of the destination CAN controller.
 * @return The MB index that was cleared, or -1 if the parameters are invalid, or -2 if freeing the index fails.
 */
sint32 can_tx_mb_map_clear(uint32 src_ctrl_idx, uint32 src_filter_id, uint32 dest_ctrl_idx)
{
    uint8 mb_index;

    if ((src_ctrl_idx >= LLCG_CONFIG_MAXCTRL_COUNT) || (src_filter_id >= HSCG_CAN_CONFIG_MAXRXMB_Z1) ||
        (dest_ctrl_idx >= LLCG_CONFIG_MAXCTRL_COUNT)) {
        return CAN_TX_ERR_PARA;
    }

    mb_index = can_tx_mb_map[src_ctrl_idx][src_filter_id][dest_ctrl_idx];
    if (can_tx_mb_index_free(dest_ctrl_idx, mb_index) >= 0) {
        can_tx_mb_map[src_ctrl_idx][src_filter_id][dest_ctrl_idx] = -1;
        return mb_index;
    }

    return CAN_TX_ERR_INTERNAL;
}

/**
 * @brief Queries the TX MB index mapping for a specified source CAN controller, filter, and destination CAN controller.
 *
 * @param src_ctrl_idx The index of the source CAN controller.
 * @param src_filter_id The ID of the source filter.
 * @param dest_ctrl_idx The index of the destination CAN controller.
 * @return The MB index, or -1 if the parameters are invalid.
 */
sint8 can_tx_mb_idx_query(uint32 src_ctrl_idx, uint32 src_filter_id, uint32 dest_ctrl_idx)
{
    if (src_ctrl_idx >= LLCG_CONFIG_MAXCTRL_COUNT || src_filter_id >= HSCG_CAN_CONFIG_MAXRXMB_Z1 ||
        dest_ctrl_idx >= LLCG_CONFIG_MAXCTRL_COUNT) {
        return CAN_TX_ERR_PARA;
    }

    return can_tx_mb_map[src_ctrl_idx][src_filter_id][dest_ctrl_idx];
}

sint32 can_output_queue_send(uint32 can_bus, uint32 msg_id, uint32 cs, const uint8 *data, uint32 dataLen)
{
    Flexcan_Ip_MsgBuffType *msg;
    sint32 ret;

    if (!can_is_forward_port(can_bus)) {
        hprintf(ERROR, "not forward port, msg drop!\r\n");
        return CAN_TX_ERR_PARA;
    }

    msg = can_msg_alloc();
    if (!msg) {
        hprintf(ERROR, "alloc can msg failed!\r\n");
        return CAN_TX_ERR_NOMEM;
    }

    if (dataLen > 64) {
        dataLen = 64;
    }

    can_build_tx_msg(msg, msg_id, cs, data, dataLen);
    ret = can_txque_enqueue(can_bus, msg);
    if (ret) {
        can_msg_free(msg);
    }
    return ret;
}



/**
 * @brief 
 * 映射关系，将路由表的目的地映射为ipc用的CPU目的地
 * 这里引入一个新的映射关系: des_id <-----> cpu_id
 * 
 * des_id用于路由表中指定需要转发的目的地
 * cpu_id则是IPC所需要的
 * 
 * 
 *  路由表中的值如下：
    LLCG_ROUTETABLE_CAN_2_CAN        = (1 << 0), //des_id = 0
    LLCG_ROUTETABLE_CAN_2_ETH        = (1 << 1), //des_id = 1
    LLCG_ROUTETABLE_CAN_2_SWITCH_2   = (1 << 2), //des_id = 2
    LLCG_ROUTETABLE_CAN_2_SWITCH_3   = (1 << 3), //des_id = 3
    LLCG_ROUTETABLE_CAN_2_SWITCH_4   = (1 << 4), //des_id = 4
    LLCG_ROUTETABLE_CAN_2_SWITCH_5   = (1 << 5), //des_id = 5
    LLCG_ROUTETABLE_CAN_2_REALTIME_0 = (1 << 6),
    LLCG_ROUTETABLE_CAN_2_REALTIME_1 = (1 << 7),
    LLCG_ROUTETABLE_CAN_2_REALTIME_2 = (1 << 8),
    LLCG_ROUTETABLE_CAN_2_REALTIME_3 = (1 << 9),
    LLCG_ROUTETABLE_CAN_2_REALTIME_4 = (1 << 10),
    LLCG_ROUTETABLE_CAN_2_REALTIME_5 = (1 << 11),
    LLCG_ROUTETABLE_CAN_2_IVI        = (1 << 12),
    LLCG_ROUTETABLE_CAN_2_ADAS       = (1 << 13),
    LLCG_ROUTETABLE_CAN_2_DB         = (1 << 14)

    例如上述的 LLCG_ROUTETABLE_CAN_2_ETH 需要映射为 SWITCH_1(ipc_cpu_id)。
    
*/
static uint8 route_destination_to_ipc_cpu_map[VCAN_MAX_SUBCRIBED] = {
    0,
    SWITCH_1,
    SWITCH_2,
    SWITCH_3,
    SWITCH_4,
    SWITCH_5,
    REALTIME_0,
    REALTIME_1,
    REALTIME_2,
    REALTIME_3,
    REALTIME_4,
    REALTIME_5,
    CPU_0,      //IVI
    CPU_5,      //ADAS
    CPUMP2_0    //DB
};

/**
 * @brief 传入 LLCG_ROUTETABLE_CAN_2_ETH ，
 *        找到其bit为1，
 *        然后返回SWITCH1
*/
int utils_get_ipc_cpu_id_by_destination_id(uint8 des_id)
{
    if(des_id >= VCAN_MAX_SUBCRIBED){
        return -1;
    }

    return route_destination_to_ipc_cpu_map[des_id];
}

int utils_get_destination_id_by_ipc_cpu_id(uint8 pid)
{
    int des_id = -1;
    for(int i=0;i < VCAN_MAX_SUBCRIBED;i++) {
        if(route_destination_to_ipc_cpu_map[i] == pid) {
            des_id = i;
            return des_id;
        }
    }

    return des_id;
}

/**
 * @brief 
 *  同上，将des_id 映射为consumer_mask
*/
static uint32 route_destination_to_consumer_mask_map[VCAN_MAX_SUBCRIBED] = {
    0,
    MCU_CORE1_MASK,
    MCU_CORE2_MASK,
    MCU_CORE3_MASK,
    MCU_CORE4_MASK,
    MCU_CORE5_MASK,
    MCU_RT0_MASK,
    MCU_RT1_MASK,
    MCU_RT2_MASK,
    MCU_RT3_MASK,
    MCU_RT4_MASK,
    MCU_RT5_MASK,
    MCU_IVI_MASK,
    MCU_ADAS_MASK,
    MCU_DB_MASK
};


/**
 * @brief 传入 LLCG_ROUTETABLE_CAN_2_ETH ，
 *        找到其bit为1，
 *        然后返回MCU_CORE1_MASK
*/
int utils_get_consumer_mask_by_destination_id(uint8 des_id)
{
    if(des_id >= VCAN_MAX_SUBCRIBED){
        return -1;
    }

    return route_destination_to_consumer_mask_map[des_id];
}

int utils_get_destination_id_by_consumer_mask(uint8 consumer_mask)
{
    int des_id = -1;
    for(int i=0;i < VCAN_MAX_SUBCRIBED;i++) {
        if(route_destination_to_consumer_mask_map[i] == consumer_mask) {
            des_id = i;
            return des_id;
        }
    }

    return des_id;
}
